/*
 * Copyright (C) 2012 Google Inc.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

#include "google_mapping/Map.h"

#include <octomap_ros/conversions.h>
#include <pcl/ros/conversions.h>

namespace google_mapping {

Map::Map(float resolution) : octree_(resolution) {}

void Map::insertPointCloud(
    const pcl::PointCloud<pcl::PointXYZ>& point_cloud,
    const tf::Pose& pose) {
  octomap::Pointcloud octomap_point_cloud;
  octomap::pointcloudPCLToOctomap(point_cloud, octomap_point_cloud);
  octomath::Pose6D octomap_pose = octomap::poseTfToOctomap(pose);
  // The point cloud should be in the sensor frame.
  octomap::point3d sensor_origin(0.f, 0.f, 0.f);
  octree_.insertScan(octomap_point_cloud, sensor_origin, octomap_pose);
}

double Map::getResolution() const {
  return octree_.getResolution();
}

double Map::getProbability(double x, double y, int level) const {
  int searchDepth = octree_.getTreeDepth() - level;
  octomap::OcTreeNode* node = octree_.search(x, y, 0.f, searchDepth);
  if (node != NULL) {
    return static_cast<float>(node->getOccupancy());
  }
  return -1.f;
}

double Map::getProbability(double x, double y, double z, int level) const {
  int searchDepth = octree_.getTreeDepth() - level;
  octomap::OcTreeNode* node = octree_.search(x, y, z, searchDepth);
  if (node != NULL) {
    return static_cast<float>(node->getOccupancy());
  }
  return -1.f;
}

Eigen::Vector3f Map::get2DInterpolatedProbabilityWithDerivatives(
    double x, double y, double z, int level) const {
  double step = octree_.getResolution() * (1 << level);
  double lower_left_center_x = (std::floor(x / step - 0.5) + 0.5) * step;
  double lower_left_center_y = (std::floor(y / step - 0.5) + 0.5) * step;

  // Now we do bilinear interpolation of the map value at (x, y) and use that
  // to calculate the derivatives at (x, y).
  //
  // x0, x1, y0, and y1 specify the centers of the surrounding cells whose
  // values are used for interpolation.
  //
  // +--------+--------+
  // | x0, y1 | x1, y1 |
  // +--------+--------+
  // | x0, y0 | x1, y0 |
  // +--------+--------+
  double x0 = lower_left_center_x;
  double x1 = lower_left_center_x + step;
  double y0 = lower_left_center_y;
  double y1 = lower_left_center_y + step;
  double M00 = getProbability(x0, y0, z, level);
  double M10 = getProbability(x1, y0, z, level);
  double M01 = getProbability(x0, y1, z, level);
  double M11 = getProbability(x1, y1, z, level);

  // Treat unknown space as empty space for the purposes of interpolation.
  M00 = (M00 == -1 ? 0. : M00);
  M10 = (M10 == -1 ? 0. : M10);
  M01 = (M01 == -1 ? 0. : M01);
  M11 = (M11 == -1 ? 0. : M11);

  return Eigen::Vector3f(
      // Interpolated value.
      ((y - y0) * ((x - x0) * M11 + (x1 - x) * M01) +
      (y1 - y) * ((x - x0) * M10 + (x1 - x) * M00)) /
      ((y1 - y0) * (x1 - x0)),
      // x derivative.
      ((y - y0) * (M11 - M01) + (y1 - y) * (M10 - M00)) /
      ((y1 - y0) * (x1 - x0)),
      // y derivative.
      ((x - x0) * (M11 - M10) + (x1 - x) * (M01 - M00)) /
      ((y1 - y0) * (x1 - x0)));
}

void Map::getOccupancyGrid(nav_msgs::OccupancyGrid* message) {
  double resolution = getResolution();
  int width, height, unused;
  getPixelSize(&width, &height, &unused);
  double minX, minY, minZ;
  getMetricMin(&minX, &minY, &minZ);
  double maxX, maxY, maxZ;
  getMetricMax(&maxX, &maxY, &maxZ);

  message->info.resolution = resolution;
  message->info.origin.position.x = minX;
  message->info.origin.position.y = minY;
  message->info.origin.orientation.w = 1.0;
  message->info.width = width;
  message->info.height = height;
  message->data.resize(width * height);

  size_t i = 0;
  for (double y = minY; y < maxY; y += resolution) {
    for (double x = minX;
         x < maxX && i < message->data.size();
         x += resolution) {
      double p = getProbability(x, y, 0);
      int value = 0;
      if (p < 0) {
        value = -1;
      } else if (p > getThreshold()) {
        value = 100;
      }
      message->data[i] = value;
      ++i;
    }
  }
}

void Map::setHitProbability(double hit_probability) {
  octree_.setProbHit(hit_probability);
}

void Map::setMissProbability(double miss_probability) {
  octree_.setProbMiss(miss_probability);
}

void Map::setMinimumOccupancy(double minimum_occupancy) {
  octree_.setClampingThresMin(minimum_occupancy);
}

void Map::setMaximumOccupancy(double maximum_occupancy) {
  octree_.setClampingThresMax(maximum_occupancy);
}

void Map::reset() {
  octree_.clear();
}

double Map::getThreshold() const {
  return octree_.getOccupancyThres();
}

void Map::getMetricMin(double* x, double* y, double* z) {
  octree_.getMetricMin(*x, *y, *z);
}

void Map::getMetricMax(double* x, double* y, double* z) {
  octree_.getMetricMax(*x, *y, *z);
}

void Map::getPixelSize(int* x, int* y, int* z) {
  double metricX, metricY, metricZ;
  octree_.getMetricSize(metricX, metricY, metricZ);
  *x = metricX / getResolution();
  *y = metricY / getResolution();
  *z = metricZ / getResolution();
}

}  // namespace google_mapping

